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ABSTRACT 


Tracking  multiple  targets  in  a  cluttered  environment  is  extremely  difficult. 
Traditional  approaches  generally  use  simple  techniques  that  combine  gating  with  some 
form  of  nearest  neighbor  association  to  reduce  the  effects  of  clutter.  When  clutter 
densities  increase,  these  traditional  algorithms  fail  to  perform  well.  To  counter  this 
problem,  the  multi-hypothesis  tracking  (MHT)  algorithm  was  developed.  This  approach 
enumerates  almost  every  conceivable  combination  of  measurements  to  determine  the 
most  likely  tracks.  This  process  quickly  becomes  very  complex  and  requires  vast 
amounts  of  memory  in  order  to  store  all  of  the  possible  tracks. 

To  avoid  this  complexity,  more  sophisticated  single  hypothesis  data  association 
techniques  have  been  developed,  such  as  the  probabilistic  data  association  filter  (PDAF). 
These  algorithms  have  enjoyed  some  success,  but  do  not  take  advantage  of  any  future 
data  to  help  clarify  ambiguous  situations. 

On  the  other  hand,  the  probabilistic  multi-hypothesis  tracking  (PMHT)  algorithm, 
proposed  by  Streit  and  Luginbuhl  in  1995,  attempts  to  use  the  best  aspects  of  the  MHT 
and  the  PDAF.  In  the  PMHT  algorithm,  data  is  processed  in  batches,  thereby  using 
information  from  before  and  after  each  measurement  to  determine  the  likelihood  of  each 
measurement-to-track  association.  Furthermore,  like  the  PDAF,  it  does  not  attempt  to 
make  hard  assignments  or  enumerate  all  possible  combinations,  but  instead  associates 
each  measurement  with  each  track  based  upon  its  probability  of  association. 
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Actual  performance  and  initialization  of  the  PMHT  algorithm  in  the  presence  of 
significant  clutter  has  not  been  adequately  researched.  This  study  focuses  on  the 
performance  of  the  PMHT  algorithm  in  dense  clutter  and  the  initialization  thereof  In 
addition,  the  effectiveness  of  measurement  attribute  data  is  analyzed,  especially  as  it 
relates  to  algorithm  initialization.  Further,  it  compares  the  performance  of  this  algorithm 
to  the  nearest  neighbor,  MHT,  and  PDAF. 
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I.  INTRODUCTION 


A.  BACKGROUND 

Tracking  multiple  targets  in  a  cluttered  environment,  such  as  is  found  in  littoral 
waters,  is  an  extremely  difficult  task.  This  is  due  to  the  added  noise  which  is  caused  by 
the  closeness  of  the  ocean  floor  to  the  surface.  Furthermore,  in  littoral  waters,  there  are 
quite  often  various  man-made  structures,  which  can  cause  the  addition  of  false  target 
sonar  returns.  In  this  environment,  targets  typically  operate  at  speeds  between  2-10 
knots.  In  order  to  maintain  steerage  control,  submarines  find  it  difficult  to  operate  below 
this  minimum  velocity,  and  above  10  knots,  diesel  submarines  will  produce  an  obvious 
amount  of  noise. 

Traditional  approaches  used  to  solve  the  cluttered  environment  tracking  problem 
have  typically  employed  simple  techniques  to  determine  what  is  a  true  measurement  from 
a  target  and  what  is  not.  They  accomplish  this  by  a  combination  of  gating  (discarding 
measurements)  and  some  form  of  nearest  neighbor  association  (picking  the  closest 
measurement  to  the  current  target  position  estimate,  where  “closesf  ’  is  defined  by  a 
weighted  distance).  In  the  open  ocean,  where  the  water  is  deep,  traditional  approaches 
perform  well  because  the  problem  of  dense  clutter  is  not  encountered.  However,  in 
littoral  waters,  where  clutter  densities  increase,  these  traditional  algorithms  fail  to  yield 
reliable  results.  In  order  to  solve  the  clutter  problem,  two  algorithms  were  developed — 
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the  multi-hypothesis  tracking  (MHT)  algorithm  and  the  probabilistic  data  association 
filter  (PDAF). 

1.  The  Multi-Hypothesis  Tracker  (MHT) 

The  MHT  [Ref.  5]  enunterates  almost  all  of  the  possible  combinations  of 
measurement-to-track  assignments.  Then  from  all  of  these  possibilities,  the  most  likely  is 
selected  as  the  best  estimate  of  the  track.  The  problem  quickly  becomes  extremely 
complex  as  the  data  combinations  are  growing  exponentially  as  each  new  measurement 
batch  is  received.  This  requires  a  huge  amount  of  memory  and  computing  power. 
Furthermore,  as  the  number  of  possibilities  increase,  some  form  of  pruning  must  be  done 
in  order  to  keep  the  number  of  hypotheses  within  limits. 

2.  The  Probabilistic  Data  Association  Filter  (PDAF) 

On  the  other  hand,  the  PDAF  [Ref.  4]  does  not  make  hard  measurement-to-track 
assignments,  but  rather  weights  each  measurement  based  upon  its  likelihood  of 
association  with  a  track.  This  algorithm  has  some  advantages  in  its  simplicity,  especially 
in  computational  and  storage  costs.  However,  it  only  gets  one  chance  to  weight  the  data 
correctly.  Therefore,  this  algorithm  does  not  take  advantage  of  any  future  data  before 
making  a  decision  on  the  most  likely  true  measurement. 

B.  A  NEW  APPROACH 

In  1995,  Streit  and  Luginbuhl  of  the  Naval  Undersea  Warfare  Center  proposed  a 
ne\\'  algorithm  called  the  probabilistic  multi-hypothesis  tracking  (PMHT)  algorithm  [Ref 
1].  Like  the  MHT,  this  algorithm  processes  data  in  batches,  thereby  giving  it  the 
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advantage  of  future  data  before  decisions  are  made.  However,  the  PMHT  does  not 
attempt  to  enumerate  all  possible  combinations,  but  rather  weights  the  measurements 
based  on  the  likelihood  of  each  measurement  being  the  true  measurement.  Therefore,  like 
the  PDAF,  this  new  algorithm  employs  an  empirical,  Bayesian  data  association  to  score 
the  measurements  in  order  to  determine  the  likely  true  measurement  centroid.  This 
technique  can  be  significantly  faster  than  the  MHT,  but  will  require  more  time  to 
compute  than  the  PDAF.  This  research  focuses  on  two  primary  areas  of  the  PMHT  that 
have  yet  to  be  studied  carefully,  that  is,  the  actual  performance  of  the  algorithm  in  the 
presence  of  dense  clutter  and  the  initialization  thereof. 

Furthermore,  this  thesis  also  addresses  the  performance  of  the  PMHT  as  compared 
to  the  traditional  tracking  algorithms — ^the  MHT  and  PDAF.  In  addition,  measurement 
attribute  data  is  explored  in  conjunction  with  the  PMHT  algorithm. 

C.  THESIS  OUTLINE 

This  thesis  is  divided  into  the  following  chapters:  Chapter  II  describes  the  theory 
and  derivation  behind  the  PMHT  algorithm.  Chapter  III  lays  out  the  explicit  algorithm  as 
it  has  been  implemented  in  this  study.  Chapter  IV  covers  the  difficulty  of  initializing  this 
algorithm  in  the  presence  of  clutter,  and  how  the  initialization  was  eventually 
accomplished.  Chapter  V  shows  the  results  of  this  implementation  of  the  PMHT 
algorithm  and  compares  these  results  to  other  tracking  algorithms,  i.e.,  the  nearest 
neighbor,  PDAF,  and  MHT.  Portions  of  these  results  were  published  and  presented  at  the 
Asilomar  Conference  on  Signals,  Systems,  and  Computers  in  November  of  1996  [Ref  2]. 
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Finally,  Chapter  VI  summarizes  this  study  and  offers  suggestions  about  areas  in  which 
further  research  might  be  conducted. 
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II.  THEORETICAL  BASIS  OF  THE  PMHT  ALGORITHM 


A.  TARGET  MOTION  MODEL 


In  this  research,  measurements  are  obtained  from  a  sensor  in  batches  at  a  set  time 
interval.  The  PMHT  algorithm  takes  all  of  the  received  measurements  and  computes  an 
optimal  estimate  for  each  target  track.  The  targets  are  assumed  to  be  independent  with 
linear  Gaussian  statistics  of  the  following  form: 

(2.1) 

where  x  is  the  state-space  vector  containing  the  x  position,  x  velocity,  y  position,  andy 
velocity,  respectively.  O  is  the  following  discrete  state-space  matrix: 


1  At  0  0 

0  10  0 
0  0  1  At 

0  0  0  1 


(2.2) 


Further,  w,  is  white  Gaussian  noise  with  known  covariance  matrix  Q,  which  is  given  by: 


Q  =  q 


'AtV3  AtV2 
At^  /  2  At 
0 


0 

0 


0 


0  0 

0  0 

AtV3  At'/2 
At^/2  At 


(2.3) 


where  At  is  the  time  in  between  scans,  and  ^  is  a  parameter  which  reflects  the 
maneuvering  behavior  of  the  target.  This  parameter  is  used  to  adjust  the  performance  of 
the  Kalman  Filter.  Usually  for  fairly  straight  tracks,  g  is  set  to  a  small  but  nonzero  value 
in  order  to  prevent  covariance  collapse  in  the  Kalman  algorithm. 
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B. 


MEASUREMENT  MODEL 


The  measurement  model  in  this  research  assumes  that  a  sensor  returns  range  and 
bearing  information  which  contains  additive  Gaussian  noise.  Therefore,  each 
measurement  pair  is  of  the  following  form: 


z„  -  +  e,  (2.4) 

[frA 

where  the  subscript  r  denotes  the  index  for  measurements  within  a  scan  {r-  1 , . .  and 

the  subscript  t  specifies  the  discrete  time  index  {t  =  0,...,7).  Hence,  there  are  total 
measurements  taken  at  time  /,  and  there  are  T  total  scan  times  in  the  scenario.  The  error 
vector  is  additive  Gaussian  noise  with  zero  mean  and  covariance  given  by: 


R  = 


(2.5) 


For  this  research,  =  100  meters  and  a,,  =  3  degrees  was  assumed.  In  this  case,  with  the 
coordinates  in  polar  form,  the  measurement  covariance  matrix  is  the  same  for  all 
measurements  at  all  time  scans.  However,  if  the  measurements  were  in  another 
coordinate  system  (e.g.,  Cartesian)  then  each  measurement  would  have  its  own 
co\'ariance  matrix. 


C.  COORDINATE  CONVERSION 

The  Kalman  Smoother  requires  a  linear  state  equation  and  a  linear  measurement 
equation.  The  bearing  measurement  in  polar  coordinates  is  nonlinear.  Therefore,  in  order 
to  use  these  measurements  in  the  classical  Kalman  Smoother,  it  is  necessary  to  convert 
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them  into  Cartesian  coordinates.  Lerro  and  Bar-Shalom  have  demonstrated  that 
converting  range-bearing  measurements  into  Cartesian  space  prior  to  implementing  the 
Kalman  algorithm  is  superior  to  utilizing  the  raw  range-bearing  measurements  directly  in 
the  Extended  Kalman  algorithm  [Ref.  3]. 

Lerro  and  Bar-Shalom  recommend  converting  the  measurements  to  Cartesian 
coordinates  using  their  “debiased”  equations.  These  equations  convert  both  the 
measurement  itself  and  its  associated  covariance  matrix.  The  following  are  the  debiased 
measurement  conversions: 


r„cos(<^J[l-(e-"^-e-"^''^)] 
r„sin(^J[l-(e-"^-e-"^''^)] 

Furthermore,  the  corresponding  covariance  matrix  is  given  by: 


(2.6) 


(2.7) 


where, 


i?,,  =  [cos^^„  (cosh2crJ  -  coshcrj )  +  sin^  (sinh2cr^  -  sinhcrj )] 

+  [cos^^„ (2cosh2frJ  - coshcxj )  +  sin^  (2 sinh2crj  -  sinhcr^ )] 

=  sin^„cos{Zi„e'^'''  +  {r^  +  al  )(1  -  )] 

i?22  =  [sin^^„  (cosh2crJ  -  coshcrj  )  +  cos^  (sinh2<Tj  -  sinh crj )] 

+  <jI e'^^'*  [sin^^„  (2cosh2crJ  -  coshcrj )  +  cos^  (2  sinh2(j^  -  sinhcrj )] 


With  these  equations,  there  is  a  different  covariance  matrix  for  each  measurement  at  each 
different  time  scan. 
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D. 


ITERATIONS 


Each  iteration  of  the  PMHT  algorithm  begins  with  a  set  of  track  position  estimates 
and  a  set  of  measurement  probabilities.  Then  the  weights  are  computed  and  centroids 
formed.  These  centroids  are  then  used  in  the  Kalman  Smoother  to  update  the  track 
position  estimates.  With  the  new  estimates,  a  new  set  of  weights  is  computed.  These 
weights  will  be  similar  to  the  previous  weights,  but  they  will  be  different  because  of  the 
new  estimates.  After  some  iterations,  the  weights  will  converge.  When  convergence  is 
reached,  the  current  estimate  is  theoretically  the  optimal  estimate  for  a  given  track. 

E.  WEIGHTING  THE  MEASUREMENTS 

In  this  research,  two  different  models  were  used  to  assign  weights  to 
measurements.  The  first  is  the  target  track  model,  which  uses  a  normal  distribution  to 
assign  weights  to  measurements.  The  second  is  the  clutter  model,  which  uses  a  constant 
value  to  assign  weights  to  the  measurements. 

1.  Track  Model 

Given  the  measurements  and  an  initial  estimate  at  each  time  scan,  the  PMHT  uses 
a  normal  distribution  betw'een  the  estimate  and  each  measurement  to  determine  the  value 
of  the  weight  assigned  to  each  measurement  in  a  given  scan.  This  weight  specifies  the 
likelihood  that  this  measurement  belongs  to  a  particular  track  model.  From  these  weights 
and  measurements,  a  centroid  is  computed  for  each  track  model.  The  centroid  is 
calculated  by  simply  multiplying  each  measurement  by  its  associated  weight  and  then 
summing  all  of  these  together. 
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The  weight  calculated  for  each  measurement  is  conditioned  on  the  position  of  the 
estimate  and  its  covariance  matrix.  If  a  given  measurement  is  far  from  the  estimate,  it 
will  get  a  low  weight.  On  the  other  hand,  if  a  measurement  is  near  the  estimate,  it  will 
receive  a  high  weight. 

2.  Clutter  Model 

Since  uniform  clutter  is  assumed  for  this  research,  the  clutter  weight  is  equal  to  a 
constant,  which  can  be  adjusted.  The  clutter  weight  was  initially  determined  by 
calculating  the  area  of  interest  for  which  measurements  will  be  returned.  The  inverse  of 
this  area  was  then  used  as  the  starting  point  for  the  clutter  weight  value.  For  optimal 
performance,  a  clutter  weight  value  an  order  of  magnitude  less  than  this  value  was  used. 


F.  KALMAN  SMOOTHING 

The  estimates  at  each  scan  are  linked  together  by  the  Kalman  Smoother.  The 
centroids  at  each  time  scan  are  used  as  the  “measurements”  in  the  Kalman  Smoother. 

This  produces  a  new  set  of  estimates  for  each  track  model.  The  Kalman  Smoother  is  used 
so  that  all  estimates  are  updated  using  all  the  available  information  from  before  and  after 
each  time  scan.  This  produces  the  best  estimate  possible  at  each  time  scan. 

If  the  conventional  Kalman  is  used,  then  each  measurement  will  have  its  own 
corresponding  covariance  matrix.  This  covariance  matrix  is  used  both  in  the  Kalman 
Smoothing  step,  as  well  as  in  the  weighting  step.  This  not  only  complicates  the 
computations  and  requires  more  memory  storage,  but  also  causes  both  the  position 
estimate  and  its  associated  covariance  matrix  to  have  to  converge  in  the  iteration  process. 
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On  the  other  hand,  if  the  Extended  Kalman  is  used,  the  same  covariance  matrix  is 
used  for  all  measurements  at  all  time  increments  not  only  in  computing  the  weights,  but 
also  in  the  smoothing  process.  This  allows  simplification  of  the  computations,  and 
during  the  iteration  process,  only  the  position  estimate  is  being  refined  and  convergence 
is  more  easily  achieved. 
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III.  THE  PMHT  ALGORITHM  IN  EXPLICIT  FORM 


A.  THE  BASIC  ALGORITHM 

This  algorithm  is  taken  from  Streit  and  Luginbuhl  [Ref.  1]  using  the  linear 
Gaussian  case.  In  section  B,  I  will  discuss  the  modifications  which  were  made  to  the 
original  algorithm,  initially.  Then  in  section  C,  I  will  cover  the  modifications,  which 
further  improved  the  performance  of  the  algorithm. 

1.  Initialization 

Measurement  probabilities,  11  }  must  be  assigned  so  that;r^^  >  0 .  It  is 

not  critical  what  values  are  assigned  to  these  measurement  probabilities  because  in  the 
first  iteration  they  will  be  recalculated.  Moreover,  they  do  not  have  an  adverse  effect 
before  they  are  recomputed.  The  tc^  values  specify  the  estimated  probability  that  a 
measurement  at  scan  t  is  assigned  to  target  model  m  after  i  iterations  of  the  PMHT 
algorithm. 

An  initial  target  state  . . . ,  )  for  each  time  increment  and  each  of  the 

M  target  models  must  be  assigned.  My  experience  has  showm  that  these  initial  estimates 
must  be  fairly  accurate  to  ensure  that  the  algorithm  performs  satisfactorily  as  clutter 
densities  increase. 

In  this  paper,  m  specifies  the  target  model  {m  =  t  specifies  the  discrete 

time  index  {t  =  0,...,7);  r  specifies  the  index  for  measurements  within  a  scan  {r  = 
and  the  superscript  i  specifies  the  iteration  index  {i  =  0,1,...). 
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2.  Computation  of  the  Weights 

For  e\'er>'  target  and  measurement  combination  at  each  scan,  a  weight  is 


computed.  The  value  of  the  likelihood  function  (assuming  normal  distribution)  evaluated 
at  the  error  between  the  current  estimated  position  and  each  measurement  is  used  for  the 
weight: 


w, 


•(i+1) 


.T-1 


(3.2) 


where. 


z^'’  =  z 

^wfr  ^ir 


,j(/) 


■  Z  —  C 

^  tm  ^  fm 


(3.3) 


is  the  error  between  the  current  estimate  and  a  measurement.  Further,  S  is  the  weighting 
matrix  defined  as: 


^/m  =  +  R,„,  (3.4) 

Here  P,„  is  the  covariance  matrix  associated  with  the  target  state  estimate,  and  C,m  is  the 
measurement  matrix  defined  as: 


C  =  C  = 

ini 


10  0  0 
0  0  10 


for  the  standard  Kalman  algorithm. 


(3.5) 
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3.  Calculation  of  the  Measurement  Centroids 

First,  the  mean  measurement  weight  for  each  target  model  m  at  time  t  is  defined 


=  -L  y 

^mt  n,  ^  ^mtr 

r=i 


Next,  the  measurement  centroid  is  computed  as: 

-o+i)  ^  — 1 —  T  (3.7) 

This  measurement  centroid  will  be  used  in  the  Kalman  Smoothing  step  below. 

4.  Target  Measurement  Probabilities  Update 

The  next  step  before  the  Kalman  Smoother  is  to  update  the  target  measurement 
probabilites.  This  is  computed  as: 

^(Hi)  ^  (3.8) 

'*'tm  nU  tm  ^  ' 

5.  Target  State  Sequences  Update 

The  target  state  sequences  are  updated  via  the  Kalman  Smoother  using  the 
measurement  centroids  as  the  inputs.  First,  the  intermediate  variables  of  the  forward 
recursion  are  initialized  as: 


(/) 

YoiO  =Am 

(3.9) 

5 

11 

(3.10) 

Here  with  these  dummy  variables,  the  model  m  and  iteration  index  i  have  been 
suppressed  for  notational  simplicity.  The  forward  recursion  is  defined  for  t  =  0,l,...,r-l 
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P,,.,,  =0P,„,,0^+Q 

(3.11) 

P  \n  P  -4-R  1  ^ 

(3.12) 

(3.13) 

(3.14) 

Then  the  updated  target  state  estimate  for  model  m  at  time  t  is; 

^V=yTj  (3.15) 

and  the  updated  target  state  estimates  for  /  =  are  computed  via  the  backward 

recursion  as: 


=  y,„  +  -  oy,„]  (3.16) 

The  equations  in  this  subsection  make  up  a  bank  of  M  Kalman  Smoothers  which  can  be 
run  in  parallel,  although  these  filters  are  not  independent  because  they  are  linked  by  the 
weights.  In  these  equations,  O  is  the  same  as  was  defined  in  (Eqn.  2.2). 

Therefore,  a  block  diagram  of  the  basic  algorithm  is  shown  below  in  Figure  3.1 . 
The  convergence  block  will  be  discussed  in  the  next  section. 


iterate 
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B.  INITIAL  MODIFICATIONS 


Several  modifications  and  additions  are  necessary  in  order  for  the  PMHT 
algorithm  to  begin  working. 

1.  Clutter  Weight  Model 

First,  there  needs  to  be  a  model  for  clutter  weights.  Since  uniform  clutter  is 
assumed  for  this  problem,  I  used  a  clutter  weight  equal  to  a  constant,  which  could  be 
adjusted.  Therefore, 

(3-17) 

was  used  for  the  clutter  model  and  (Eqn.  3.1)  was  used  for  target  track  models. 

2.  Convergence  Criteria 

The  basic  algorithm  does  not  specifically  state  how  convergence  is  to  be 
determined.  In  this  research,  convergence  was  measured  by  the  ;r  values.  It  would  also 
be  possible  to  test  convergence  through  the  weights.  However,  both  approaches  yield 
similar  results  and  the  ;r  values  are  quicker  to  sum  and  compare.  Therefore,  initially 
convergence  was  achieved  when 

T  M  i  , 

(3.18) 

The  parameter  Kc  is  adjusted  for  optimal  performance.  If  this  number  is  set  too 
high,  then  the  algorithm  will  stop  before  it  has  fully  converged  to  its  best  solution.  On 
the  other  hand,  if  this  number  is  set  too  low,  then  the  algorithm  might  never  be  able  to 
meet  this  criteria.  Initially  Kc  was  set  to  10"^.  Iterations  are  allowed  to  continue  until 
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convergence  is  reached  or  the  maximum  limit  is  exceeded  (100  iterations).  I  will  refer  to 
this  convergence  parameter  as  the  stopping  criteria  in  the  sections  below, 

3.  Measurement  Covariance 

Using  the  basic  algorithm  in  Cartesian  Coordinates  requires  that  each 
measurement  have  its  own  associated  covariance  matrix.  For  the  weighting  equation 
(Eqn.  3.1),  it  is  possible  to  use  each  measurement’s  associated  covariance  matrix. 

Howe\  er,  during  the  Kalman  Smoothing  step,  a  covariance  matrix  is  needed  for  each 
track  at  each  time  scan.  Furthermore,  since  the  measurement  centroids  are  mixtures  of 
the  received  measurements,  there  is  not  just  one  matrix  for  each  track  model  at  each  scan. 
The  following  variations  were  investigated  during  this  research: 

a.  Weighted  Covariance 

For  this  calculation,  a  weighted  covariance  matrix  is  obtained  by  a  similar 
process  as  is  used  to  obtain  the  measurement  centroids. 


Ri::  =  s- 


Vi', 


,(') 


— R 


(3.19) 


b.  Closest  Measurement 

The  covariance  matrix  associated  with  the  measurement  which  is  closest  to 
the  current  estimate  is  used. 


(3.20) 

c.  Covariance  of  the  Estimate 

Here,  the  covariance  matrix  is  computed  from  the  current  estimated 
position  by  using  the  debiased  equations  (Eqn.  2.7). 
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R-’  =R(»2) 


(3.21) 


Of  these  variations,  the  estimated  covariance  (Eqn.  3.21)  has  shown  the 
most  robustness  and  has  provided  the  best  overall  results.  Furthermore,  it  has  been  found 
that  using  this  estimated  covariance  matrix  not  only  in  the  Kalman  Smoother  (Eqn.  3.12), 
but  also  in  the  computation  of  the  weights  (Eqn.  3.4),  provided  even  better  results. 

4.  Distant  Clutter 

It  was  found  that  if  a  measurement  point  was  too  far  from  the  estimated  position 
then  the  weight  calculated  was  extremely  small,  and  numerical  instabilities  would  be 
encountered.  Therefore,  on  a  suggestion  from  Dr.  Streit,  any  measurements  beyond  a 
Chi-squared  cut-off  value  of  0.995  from  the  estimated  track  position  have  their  weight  set 
to  a  low  constant  value  of  10'^°,  rather  than  using  the  actual  weight  from  (Eqn.  3.1). 

5.  Five  Scan  Batches 

The  initialization  process  (to  be  discussed  in  Chapter  IV)  produces  the  initial 
estimates  for  the  first  five  time  scans.  The  PMHT  algorithm  is  run  on  these  five  time 
scans  until  convergence  is  reached.  Then  the  next  five  time  scans  are  predicted,  and  the 
algorithm  runs  over  the  now  10  time  scans,  and  so  forth.  This  not  only  provides  a  more 
realistic  approach  to  what  might  actually  be  implemented,  but  also  allows  the  algorithm 
to  only  have  to  sort  out  five  new  points  at  a  time. 

6.  Speed  Limit 

Since  the  targets  of  concern  in  this  research  have  speeds  of  2-10  knots,  a  10  knot 
maximum  speed  is  imposed  when  predicting  ahead.  The  actual  target  estimates  produced 
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by  the  PMHT  algorithm  are  not  limited  and  can  converge  to  an  estimate  with  any 
velocit}'. 

Therefore,  a  block  diagram  with  these  modifications  is  shown  in  Figure  3.2. 


Iterate 


C.  FURTHER  MODIFICATIONS 

The  above  modifications  were  sufficient  in  order  for  the  PMHT  algorithm  to  work 
well.  \Mth  these  modifications,  the  PMHT  algorithm  was  able  to  perform  better  than  the 
PD.AF  at  clutter  densities  up  to  3.33x10  '  clutter  points  per  square  kilometer.  This  value 
for  the  clutter  density  appears  to  be  low.  However,  the  standard  deviation  for  the  range  is 
+/-I00  meters,  and  the  standard  deviation  for  the  bearing  is  +/-3  degrees.  Therefore,  with 
these  values  in  the  measurement  covariance  matrix,  this  clutter  density  is  fairly 
sisnificant. 
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On  the  other  hand,  in  order  to  increase  the  performance  of  the  algorithm  further, 
the  following  modifications  were  also  implemented  and  found  to  be  effective,  especially 
for  the  higher  clutter  densities. 

1.  Clutter  Weight  Model  Inflation 

Adjusting  the  clutter  weight  model  parameter  r  after  the  algorithm  has  sorted  out 
the  first  10  points  has  proven  beneficial.  That  is,  for  the  first  10  points,  p  is  set  to  a  value 
of  lO"'^,  and  then  it  is  increased  to  a  value  of  10'*°  for  the  rest  of  the  batches. 

2.  Extended  Kalman 

The  numerical  complications  involved  with  the  measurement  covariance  matrix 
led  to  trying  the  Extended  Kalman  algorithm.  As  has  been  discussed,  the  classical 
Kalman  requires  a  different  covariance  matrix  for  each  different  point  in  the  Cartesian 
Coordinates.  On  the  other  hand,  if  the  Extended  Kalman  is  used,  one  measurement 
covariance  matrix  is  used  for  all  models  at  all  time  scans.  Using  this  approach  has  shown 
some  significant  advantages  which  will  be  discussed  in  greater  detail  in  Chapter  V. 

Therefore,  the  changes  necessary  to  implement  the  Extended  Kalman  smoother 
are  the  following: 

a.  Coordinate  Conversion 

First,  the  innovations  equation  (Eqn.  3.3)  must  incorporate  the  coordinate 

conversion. 

(3-22) 

where. 
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g(x)  = 


(3.23) 


atan(7) 

b.  The  Measurement  Matrix 
The  Kalman  measurement  matrix  (Eqn.  3.5)  will  no  longer  be  a  constant 
matrix,  but  will  have  to  be  re-computed  each  time. 


The  same  modifications  are  also  necessary  for  the  smoothing  step. 
Therefore,  (Eqns.  3.1 1-3.16)  will  be  changed  accordingly.  Another  effect  of  using  the 
Extended  Kalman  is  that  the  algorithm  converges  more  quickly  and  easily  than  before. 
Therefore,  the  stopping  criteria,  Kc,  can  be  lowered,  producing  relatively  the  same 
number  of  iterations,  while  allowing  a  tighter  convergence  parameter.  For  the  Extended 
Kalman  algorithm,  Kc  was  set  to  10  *. 

3.  Stricter  Speed  Limit 

As  different  geometries  were  simulated,  I  found  that  the  original  speed  limit  still 
allowed  targets  to  “run  away,”  i.e.,  have  a  velocity  estimate  which  was  too  fast. 
Therefore,  a  modification  to  the  limit  on  the  velocity  of  the  predicted  track  estimate  was 
necessary. 

The  stricter  speed  limit  is  implemented  by  first  selecting  the  middle  time  scan 
estimate  as  the  baseline.  If  this  estimate  has  a  speed  in  excess  of  10  knots,  then  the 
velocit}-  of  the  baseline  is  reduced  to  reflect  a  speed  of  10  knots.  Then  the  state  estimates 
at  both  past  and  future  times  are  generated  from  the  adjusted  baseline  state  with  its  new 
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refined  velocity.  These  estimates  are  then  used  in  the  next  batch  of  processing.  Even 
with  this  stricter  speed  limit,  the  algorithm  is  still  allowed  to  converge  to  a  final  track 
estimate  with  any  velocity. 

4.  Attribute  Data 

The  use  of  measurement  attribute  data  was  explored  and  added  to  the  algorithm. 
This  method  assumes  that  a  measure  of  target  amplitude  was  available  in  addition  to  the 
range-bearing  measurement.  The  measured  amplitude  is  drawn  from  a  Rayleigh 
distribution  with  specified  mean,  which  has  the  form: 

f(a)  =  (a/cr^)exp(-a^ /2(7^),  a  >  0  (3.25) 


The  mean  for  this  distribution  is  a^n  12  .  For  this  research,  targets  and  clutter  are 
assigned  different  means.  The  clutter  model  is  given  a  mean  with  a^l,  and  the  target 
models  have  means  with  a>\.  This  data  is  included  by  multiplying  the  weights  found  in 
(Eqns.  3.1  &  3.17)  by  the  Rayleigh  distribution.  Therefore,  (Eqns.  3.1  &  3.17)  become 


, ‘('+1)  _ 


=  f(a)- 


^  tm  ^mtr  j 


2;rVdet(E,„,) 


(3.26) 


=  mp  (3-27) 

respectively.  This  modification  was  only  used  in  the  simulations  that  are  indicated  in 
Chapter  V. 

A  block  diagram  with  all  of  the  modifications,  except  the  attribute  data,  is  shown 


in  Figure  3.3. 


iterate 


This  concludes  the  layout  of  the  PMHT  algorithm,  which  is  utilized  in  this 
research.  In  the  next  chapter,  I  will  discuss  the  initialization  routine  that  is  used  for  this 
algorithm. 
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IV.  TARGET  STATE  INITIALIZATION 


A.  GENERAL  DISCUSSION 

The  initialization  algorithm  used  in  this  research  is  of  the  N-of-N  variety.  That  is, 
for  the  first  N  time  scans,  there  must  be  N  measurement  points  which  meet  the  gating 
criteria.  The  PMHT  algorithm  is  very  sensitive  to  the  initial  estimates  it  is  given.  For 
small  values  of  N,  the  initial  heading  can  be  extremely  inaccurate,  and  then  the  algorithm 
is  less  likely  to  converge  to  the  true  track.  Therefore,  I  have  found  that  values  of  V  >  5 
are  necessary  for  the  PMHT  algorithm  to  perform  reliably. 

B.  THE  N-of-N  INITIALIZATION  ALGORITHM 

There  are  two  steps  for  this  process  when  N  >3 .  The  first  step  is  a  gating 
equation  which  eliminates  all  points  which  are  not  likely  to  be  associated  as  a  track. 

Then  once  a  set  of  N  points  has  been  designated  as  a  new  track,  a  least  squares  algorithm 
is  used  to  fit  the  optimal  track  to  these  N  points. 

1.  Gating 

As  has  been  stated  before,  this  research  assumes  that  targets  have  a  speed  between 
2  and  10  knots.  If  the  target  velocity  vector  is  known  and  two  successive  measurements 
(z,  and  Z/+i)  are  obtained  for  the  target,  then  the  quantity 

-z, )’■[«„,  +R,l''(z,.,  -Z,)  (4.1) 


23 


has  a  non-central  Chi-squared  distribution,  where  z,  and  R,  are  the  measured  track 
position  and  measurement  covariance  matrix  at  time  t.  It  was  empirically  demonstrated 
that  for  the  target  speeds,  tracking  ranges,  and  measurement  errors  used  in  this  research,  a 
cut-off  of  ^  50  kept  all  target  associations.  This  cut-off  produces  an  effective  gate  for 
2-of-2  association  of  approximately  75  square  kilometers.  A  3-of-3  case  relies  on  a 
match  (using  the  2-of-2  algorithm)  for  the  first  and  second  measurements  and  the  second 
and  third  measurements.  Then  a  cut-off  value  for  track  formation  was  applied  with 

X*  -  20 .  This  gives  an  association  probability  for  measurements  derived  from  a  real 
track  of  0.9972.  Higher  order  associations  project  the  subsequent  result  into  the  future  to 
determine  likely  measurements  for  the  new  association.  At  each  projection  for  the  next 
higher  association,  a  match  is  performed  and  then  a  rejection. 

2.  Least  Squares  Fit 

This  initialization  algorithm  uses  a  least  squares  fit  assuming  a  constant  velocity 
target  during  the  initialization  period.  Therefore,  for  the  3-of-3  case,  the  first  three 
measurements  are  given  by: 


z,„,  =Cx,„, +e,^ 


=  Cx,„,  -)-  e^,  =  C[ct)x,„,  -I-  w,„,]  -1- e^, 

*3.,  =Cx3„  -Hej,  =C[0(<bx„„  +w,„,)  +  W2„,]-he3, 


(4.2) 


Since  a  straight,  constant  velocity  target  is  assumed  during  the  initialization,  the  process 
errors  are  set  to  zero  (i.e.,  w,„,=0  for  every  t).  Therefore, 
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(4.3) 
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where  the  covariance  associated  with  the  error  vector  is 
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Therefore,  the  least  squares  estimate  of  Xi„  is  given  by: 


and 

*3.  =  (4.7) 

since  this  gives  the  estimates  for  the  position.  For  the  associated  state  estimate 
covariance  matrices,  the  following  equations  are  used: 
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P,.  =  [F^liF]-'F"([F’S'^F]-'F^S;f  (4.8) 

and 

P3.,  =  +  Q  =  +  Q  (4-9) 
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From  this  3-of-3  case,  it  is  straightforward  to  expand  this  in  order  to  apply  this  process  to 
the  5-of-5  case.  These  estimates  and  their  associated  covariance  matrices  are  then  used  in 
the  first  batch  of  the  PMHT  algorithm. 
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V.  RESULTS  AND  COMPARISONS 


A.  SIMULATIONS 

In  this  research,  all  of  the  comparisons  are  based  on  simulated  data.  Simulations 
were  run  for  30  time  scans  with  the  time  between  scans  equal  to  4  minutes.  The  30 
range-bearing  target  measurements  are  generated  using  additive  Gaussian  noise,  where 
the  range  standard  deviation  is  100  meters  and  the  bearing  standard  deviation  is  3 
degrees.  An  example  of  a  simulated  target  run  is  shovra  in  Figme  5.1. 


X  10" 


Figure  5.1 .  Straight  Track  with  Clutter 
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The  solid  line  is  the  true  track,  and  the  are  the  noisy  measurements.  The 
actual  target  velocity  is  5  knots  in  all  scenarios.  The  sensor  is  located  at  (0,0),  so  the 
target  is  moving  predominantly  in  the  cross  range  dimension  at  a  range  greater  than  40 
kilometers. 

This  example  also  depicts  clutter  points  as  circles.  Clutter  was  generated  in  a 
uniform  random  fashion  throughout  the  area  of  interest.  For  this  example  the  clutter 
density  is  1 .67x1  O  '  clutter  points  per  square  kilometer,  or  in  other  words,  there  are  five 
clutter  points  for  each  time  scan.  Clutter  densities  in  this  study  were  varied  from 
3.33x10'^  to  6.67x10  ’  clutter  points  per  square  kilometer. 

B.  OTHER  ALGORITHMS 

The  MHT  and  PDAF  algorithms  were  used  as  a  benchmark  to  see  how  well  the 
PMHT  is  performing.  As  was  mentioned  in  Chapter  I,  both  of  these  are  established 
algorithms  that  are  currently  being  used  in  tracking  applications.  The  results  from  these 
algorithms  were  produced  using  the  exact  same  scenarios  as  were  used  for  the  PMHT. 
The  MHT  and  PDAF  algorithms  used  are  widely  discussed  in  several  different  texts  (e.g. 
Bar-Shalom  and  Li  [Ref  4],  Blackman  [Ref  5]). 

C.  RESULTS 

Three  different  target  motion  geometries  were  tested  during  this  research.  First, 
there  is  the  basic  straight  line  track  as  is  depicted  in  Figure  5.1.  Second,  tvv'o  crossing 
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tracks  are  used  moving  in  straight  lines  with  constant  velocity.  The  third  geometry  is  a 
track  which  makes  a  turn  half  way  through  the  simulation. 

The  PMHT  algorithm  was  first  tested  in  each  of  the  three  geometries  with  a  low 
clutter  density  (3.33x10’^  clutter  points  per  square  kilometer).  Then  it  was  confirmed 
with  higher  clutter  densities  and  compared  to  the  competing  algorithms. 

1.  Straight  Line  Target  Motion 

Figure  5.2  displays  a  typical  converged  result  produced  by  the  PMHT  algorithm. 
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Figure  5.2.  Typical  PMHT  Result 
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On  this  single,  straight  line  track,  the  circles  are  the  actual  target  positions  at  each  time 
increment,  and  the  symbols  are  the  smoothed,  converged  estimates.  The  clutter 
density  for  this  simulation  is  3.33x10  clutter  points  per  square  kilometer, 
a.  Low  Clutter  Density 

In  order  to  quantify  the  results,  mean  distance  errors  were  computed  for 
the  final  target  estimates  at  each  time  scan.  The  means  were  taken  from  500  simulation 
runs.  Figure  5.3  shows  these  mean  distance  errors  for  a  straight  line  track  in  a  clutter 
density  of  3.33x10  ’  clutter  points  per  square  kilometer. 


Figure  5.3.  Measurement  and  Estimate  Errors  (low  clutter) 
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The  lowest  dotted  line  curve  on  the  figure  is  the  result  of  running  the 
Kalman  Smoother  using  the  actual  target  measurements  (i.e.,  no  clutter)  and  computing 
the  mean  distance  errors  over  1000  simulation  runs.  Therefore,  this  curve  represents  an 
approximate  theoretical  minimum  for  algorithm  performance  in  the  absence  of  clutter. 
The  highest  curve  on  the  figure  (the  dotted  line)  is  the  average,  noisy  measurement  error 
computed  over  1000  runs.  Clearly,  an  algorithm  should  be  below  this  line  to  be 
considered  as  a  viable  option.  The  other  line  on  the  figure  (the  solid  line)  is  the  mean 
estimate  error,  which  was  produced  by  the  algorithm.  This  is  the  average  estimate  error 
for  500  simulation  runs  at  a  clutter  density  of  3.33x10'^  clutter  points  per  square 
kilometer. 

In  this  simulation,  the  algorithm  happened  to  produce  estimate  errors 
which  have  a  better  mean  than  the  Kalman  Smoother  without  clutter.  This  can  be  true 
because  of  randomness,  but  in  general  will  not  happen.  However,  it  does  show  that  the 
PMHT  algorithm  is  performing  extremely  well  in  low  clutter,  as  would  be  expected. 

b.  Medium  Clutter  Density 

Figure  5.4  shows  the  results  using  the  PMHT  algorithm  in  a  medium 
clutter  density.  The  medium  clutter  density  has  five  clutter  points  and  a  noisy 
measurement  in  the  search  area  at  each  time  scan.  The  clutter  density  is  1.67x10'^  clutter 
points  per  square  kilometer.  The  solid  line  in  Figure  5.4  displays  the  results  from  using 
the  Extended  Kalman  Smoother  (EKS)  in  the  algorithm.  The  broken  line  shows  the 
results  of  using  the  conventional  Kalman  Smoother  in  Cartesian  Coordinates,  using  the 
debiased  equations.  As  can  be  seen,  the  Extended  Kalman  Smoother  performed  better. 
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Furthermore,  the  EKS  performed  the  500  simulations  in  only  204  minutes,  whereas,  the 
conventional  Kalman  algorithm  took  280  minutes.  Therefore,  from  these  results,  I  have 
determined  for  this  application  that  it  is  better  to  use  the  EKS  as  opposed  to  the 
conventional  Kalman  with  the  debiased  equations.  From  here  on,  I  will  only  show  results 
from  the  EKS,  but  at  other  clutter  densities,  the  results  are  similar  between  the 
conventional  Kalman  and  the  EKS,  as  is  shown  here. 


Figure  5.4.  Comparison  between  EKS  and  Conventional  Kalman 


Figure  5.5  shows  the  comparison  between  the  nearest  neighbor  (NN), 
PMHT,  MHT,  and  PDAF.  On  this  figure,  there  are  four  different  lines — one  for  each  of 
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the  different  algorithms.  The  solid  line  represents  the  PMHT;  the  solid  line  with  circles  is 
the  nearest  neighbor;  the  broken  line  is  the  MHT;  and  the  dash-dot  line  is  the  PDAF.  The 
important  estimate  error  to  compare  is  at  the  last  time  scan.  This  is  the  time  that  is 
current  and  is  of  importance.  The  PMHT  will  almost  always  have  better  errors  for  earlier 
time  scans  because  of  the  smoothing  process.  For  this  clutter  density,  the  PMHT  is 
performing  slightly  better  than  the  MHT  and  considerably  better  than  the  PDAF  and  the 
nearest  neighbor.  This  is  especially  promising  given  the  fact  that  the  PMHT  is  less 
complicated  and  easier  to  compute  than  the  MHT.  Since  the  nearest  neighbor  algorithm 
is  clearly  not  a  viable  option,  it  is  not  included  on  subsequent  plots. 


Figure  5.5.  Comparison  between  NN,  PMHT,  PDAF,  and  MHT  (medium  clutter) 


33 


c.  High  Clutter  Density 

For  a  high  density  clutter  environment,  a  clutter  density  of  3. 33x1 0  ‘ 
clutter  points  per  square  kilometer  was  used.  This  meant  that  there  were  10  clutter  points 
and  one  noisy  measurement  in  the  search  area  per  time  scan.  The  results  from  this 
simulation  are  shown  in  Figure  5.6. 


Figure  5.6.  Comparison  between  PMHT,  PDAF,  and  MHT  (high  clutter) 

These  results  show  that  the  PMHT  is  performing  better  than  the  PDAF, 
but  worse  than  the  MHT  for  this  clutter  density.  Therefore  at  this  point,  there  is  a  cost 
trade-off  versus  the  algorithm  performance.  The  MHT  performs  the  best,  but  it  also  is  the 
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most  expensive  in  terms  of  computations.  On  the  other  hand,  the  PDAF  is  producing  the 
worst  results,  but  it  is  the  cheapest  and  fastest  to  calculate. 

2.  Turning  Tracks 

For  this  scenario,  the  target  moved  in  a  straight  line  with  constant  speed  for  the 
first  60  minutes.  At  that  point,  the  target  made  a  10  degree  turn  and  then  continued  in  a 
straight  line  for  the  remaining  time.  This  target  motion  and  a  typical  converged  result 
produced  by  the  PMHT  algorithm  are  displayed  in  Figure  5.7. 
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Figure  5.7.  Typical  PMHT  Results  with  a  Turning  Track 
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Again,  the  circles  are  the  actual  target  positions  at  each  time  increment,  and  the 
symbols  are  the  smoothed,  converged  estimates.  The  clutter  density  for  this 
simulation  is  3.33x10'^  clutter  points  per  square  kilometer.  This  result  shows  that  the 
algorithm  is  not  handling  the  turn  quite  as  well  as  the  straight  line  track;  however,  it  is 
tracking  nonetheless.  For  this  10  degree  turn,  the  q  factor  was  set  to  a  value  of  one. 
a.  Low  Clutter  Density 

Figure  5.8  displays  the  mean  errors  for  a  turning  track  for  500  simulation 
runs.  The  solid  line  represents  the  estimate  errors  at  each  time  scan. 


Figure  5.8.  Measurement  and  Estimate  Errors  (low  clutter) 
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The  clutter  density  for  this  simulation  is  3.33x10'^  clutter  points  per  square 
kilometer.  While  the  errors  for  this  scenario  are  higher  than  for  the  straight  track,  they 
are  still  well  below  the  measurement  errors. 

b.  Medium  Clutter  Density 

Figure  5.9  shows  the  results  of  the  PMHT  algorithm  on  a  turning  track  in  a 
clutter  density  of  1 .67x10'^  clutter  points  per  square  kilometer.  These  results  show  that 
the  algorithm  is  still  performing  well  even  with  the  target  making  a  turn  in  the  presence 
of  a  medium  clutter  density.  This  further  justifies  the  use  of  the  q  factor  as  a  small,  but 
positive  value.  For  this  scenario,  q  was  set  to  a  value  of  10. 


Figure  5.9.  Measurement  and  Estimate  Errors  (medium  clutter,  10°  turn) 
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Figure  5. 10  shows  the  results  of  the  three  algorithms  estimating  a  turning 
track  with  a  90  degree  turn  instead  of  a  10  degree  turn.  Again,  the  solid  line  represents 
the  PMHT;  the  broken  line  is  the  MHT;  and  the  dash-dot  line  is  the  PDAF.  The  clutter 
density  is  still  1 .67x10  "  clutter  points  per  square  kilometer. 


Figure  5.10.  Comparison  between  PMHT,  PDAF,  and  MHT  (90"  turn) 


Here  the  q  factor  was  set  to  a  value  of  300  in  an  effort  to  try  and  get  the 
PMHT  algorithm  to  track  through  the  turn.  However,  with  a  turn  this  radical,  the  PMHT 
and  PDAF  algorithms  were  unable  to  maintain  the  target.  The  MHT  results  show  that  the 
target  was  lost  at  the  turn,  but  was  able  to  be  reacquired  after  the  turn.  The  MHT 
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algorithm  has  reacquisition  logic  inherent  in  the  algorithm  itself,  while  the  other  two 
algorithms  do  not  have  this  process  inherent  to  the  basic  tracking  filter.  Therefore,  this 
result  leads  to  the  conclusion  that  the  use  of  the  PMHT  will  require  that  some  sort  of 
target  re-initialization  be  implemented  in  a  fielded  system.  A  procedure  for  linking  the 
new  track  with  the  old  would  also  be  required. 

c.  High  Clutter  Density 

Figure  5.1 1  shows  the  results  of  the  PMHT  tracking  a  target  through  a  10 
degree  turn  in  a  clutter  density  of  3.33x10'^  clutter  points  per  square  kilometer. 


Figure  5.1 1.  Measurement  and  Estimate  Errors  (high  clutter,  10°  turn) 
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These  results  confirm  what  was  seen  at  the  medium  clutter  density — 
namely  that  the  PMHT  algorithm  is  able  to  robustly  follow  a  track  as  it  makes  small 
maneuvers.  This  justifies  further  using  the  small,  but  non-zero  q  factor  value  instead  of 
setting  it  to  zero,  which  would  be  the  equivalent  of  using  a  least  squares  fit  instead  of  a 
Kalman  filter  or  smoother. 

3.  Crossing  Tracks 

In  this  scenario,  there  are  two  targets  in  the  simulation.  Target  one  executes  the 
same  track  that  was  used  in  the  straight  track  from  before.  Target  two  executes  a  track 
which  starts  in  the  bottom  left-hand  comer  of  the  figure  and  mns  toward  the  upper  right- 
hand  comer.  Therefore,  this  new  track  is  predominantly  in  the  cross  bearing  direction. 
These  tracks  along  with  typical  converged  results  from  the  PMHT  algorithm  are  shov\'n  in 
Figure  5.12. 

Again,  the  actual  target  positions  are  the  circles,  and  the  symbols  are  the 
smoothed,  converged  estimates.  The  clutter  density  for  this  simulation  is  3.33x10'-'' 
clutter  points  per  square  kilometer.  Initially,  there  were  some  problems  with  the 
algorithm  tracking  the  different  trajectories,  but  since  the  improved  limit  on  velocity  was 
implemented,  this  has  not  been  a  drawback.  This  typical  result  shows  the  effect  of  the 
greater  uncertainty'  in  the  bearing  dimension.  Track  one,  which  is  predominantly  moving 
in  the  cross-range  direction,  has  a  greater  uncertainty  along  its  axis  of  travel.  On  the 
other  hand,  track  two,  which  is  predominantly  moving  in  the  cross-bearing  direction,  has 
a  greater  uncertainty  perpendicular  to  its  axis  of  travel. 
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Figure  5.12.  Typical  PMHT  Results  with  Crossing  Tracks 

a.  Low  Clutter  Density 

Figure  5.13  displays  the  results  from  the  crossing  track  scenario  with  a  low 
clutter  density  of  3.33x10'^  clutter  points  per  square  kilometer.  The  solid  line  shows  the 
mean  errors  from  track  one,  and  the  broken  line  is  from  track  two.  Again,  the  lowest 
dotted  line  represents  the  theoretical  minimum,  which  was  produced  by  running  the 
Kalman  smoother  with  just  the  noisy  measurements.  The  highest  line  is  the  average 
measurement  error  over  1000  simulations.  These  results  are  still  below  the  measurement 
error,  but  significantly  higher  than  the  single  track  in  a  low  clutter  density. 
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Figure  5.13.  Errors  for  Crossing  Tracks  (low  clutter) 

One  reason  for  this  is  the  effect  that  each  track  has  on  the  other  one.  The 
clutter  has  an  equal  probability  of  affecting  the  estimate  to  one  side  or  the  other,  and,  in 
general,  clutter  will  tend  to  have  little  bias  effect  on  the  track  estimate.  On  the  other 
hand,  the  measurements  from  the  other  track  will  tend  to  pull  the  estimate  toward  these 
measurements,  causing  a  bias  to  one  side.  Since,  in  this  low  clutter  density  the  clutter  has 
minimal  effect,  this  bias  effect  of  the  other  track  is  clearly  evident.  Figure  5.14  shows 
this  bias  effect.  This  figure  is  the  average  track  estimate,  which  is  produced  by  the 
algorithm  over  the  500  runs. 
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Figure  5.14.  Average  Track  Estimate  (low  clutter) 

The  true  target  tracks  are  shown  as  the  solid  lines,  and  the  estimate 
average  is  displayed  as  the  dotted  lines.  Track  one  is  pulled  downward,  and  track  two  is 
pulled  up  towards  track  one.  In  the  next  subsection,  I  will  show  how  more  clutter  will 
dampen  out  this  bias  effect. 

b.  Medium  Clutter  Density 

Figure  5.15  shows  the  results  from  the  crossing  track  scenario  in  a  medium 
clutter  density  of  1.67x10'^  clutter  points  per  square  kilometer.  Once  again,  the  solid  line 
represents  the  mean  errors  from  track  one,  and  the  broken  line  is  from  track  two. 
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Figure  5.15.  Errors  for  Crossing  Tracks  (medium  clutter) 


Even  though  the  clutter  density  is  five  times  greater,  these  results  are 
significantly  better  than  the  low  clutter  density  case.  This  is  due  to  the  higher  clutter 
densitN-  dampening  out  the  bias  caused  by  the  two  tracks.  These  results  compare  ver>' 
closely  to  those  obtained  for  the  single  straight  track  in  a  medium  clutter  density. 

Figure  5.16  shows  the  average  track  estimate  for  this  scenario  as  was 
depicted  in  Figure  5.14  for  the  low  clutter  density.  Again,  the  true  target  tracks  are 
shown  as  the  solid  lines,  and  the  estimate  averages  are  displayed  as  the  dotted  lines. 
Indeed,  the  separate  tracks  show  veiy-  little  bias  toward  the  other  track  in  the  simulation. 
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Figure  5.16.  Average  Track  Estimate  (medium  clutter) 

c.  High  Clutter  Density 

Figure  5.17  shows  the  results  from  the  crossing  track  scenario  in  a  high 
clutter  density.  Again,  the  solid  line  displays  the  mean  error  from  track  one,  and  the 
broken  line  represents  the  mean  errors  from  track  two.  The  clutter  density  for  this 
scenario  is  3.33x10'^  clutter  points  per  square  kilometer.  For  this  simulation,  the  errors 
are  starting  to  get  up  close  to  the  measurement  errors,  and  for  clutter  densities  higher  than 
this,  the  errors  begin  to  exceed  the  measurement  errors. 
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Figure  5. 1 7.  Errors  for  Crossing  Tracks  (high  clutter) 


Another  observ'ation  from  these  results  is  the  fact  that  track  one’s  estimate 
is  higher  than  that  of  track  tw'o.  For  the  higher  clutter  densities,  this  was  found  to  be  the 
norm.  The  reason  track  one  produces  the  higher  estimate  errors  is  due  to  the  target 
motion  being  predominantly  in  the  cross-range  direction.  Therefore,  this  is  the  reason 
track  one  was  used  for  the  majority  of  the  simulations. 

4.  Attribute  Data 

The  use  of  attribute  data  was  researched  to  determine  what  level  amplitude  of 
target  data,  in  relation  to  the  clutter  data  amplitude,  was  necessary  in  order  to  improve  the 
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performance  of  the  PMHT  algorithm.  For  this  simulation,  a  very  high  clutter  density  of 
6.67x10'^  clutter  points  per  square  kilometer  was  used.  Figure  5.18  shows  the  results  of 
the  PMHT  algorithm  in  this  clutter  density  with  and  without  attribute  data. 


Figure  5.18.  Attribute  Data  Comparison  (very  high  clutter) 

The  solid  line  represents  the  algorithm  with  the  use  of  attribute  data,  and  the 

broken  line  is  the  algorithm  without  it.  For  this  simulation  an  attribute  value  of  a  =  VlO 
was  necessary  in  order  to  get  a  noticeable  improvement  from  the  algorithm.  As  was 
described  in  Chapter  III,  this  means  that  this  value  of  a  was  used  for  the  target 
measurements,  and  a  value  of  a  =  1  was  used  for  the  clutter  measurements.  For  values 
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less  than  a  =  ,  no  clear  advantage  could  be  seen.  The  fact  that  a  lOdB  power 

ad\  antage  is  needed  for  the  target  data  in  order  to  show  an  improvement  is  not  vet}' 
encouraging  at  this  point.  Given  the  findings  of  this  research,  a  large  amplitude 
separation  would  be  necessary  to  produce  any  sort  of  real  advantage  using  the  attribute 
data. 

Attribute  data  was  also  utilized  to  see  if  it  could  lower  the  requirements  for 
initialization.  Figure  5.19  shows  the  effect  of  varying  the  initialization  constant  N,  which 
is  in  .V-of-.V.  A  1  OdB  power  ratio  is  used  throughout  these  simulations. 


Figure  5.19.  Mean  Distance  Errors  with  Attribute  Data  and  Varying  N 
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The  solid  line  is  the  standard  PMHT  with  no  attribute  data  and  N=5.  The  dotted 
line  is  the  algorithm  with  attribute  data  and  N—3,  and  the  dash-dot  line  represents  attribute 
data  with  N=5.  The  clutter  density  for  these  simulations  is  a  high  clutter  density  of 
3.33x10'^  clutter  points  per  square  kilometer.  Here  the  attribute  data  with  N=3  is  not  as 
good  as  no  attribute  processing  and  N=5.  However,  attribute  data  with  N=5  shows 
superior  results.  Therefore,  at  higher  clutter  levels,  it  is  not  possible  to  reduce  N  and 
make  up  the  difference  with  lOdB  of  attribute  information. 
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VI.  CONCLUSIONS 


A.  SUMMARY 

This  research  has  explored  the  possible  implementation  and  initialization  of  the 
PMHT  algorithm.  It  has  solved  and  improved  many  of  the  aspects  of  running  the 
algorithm.  This  includes  the  development  of  an  initialization  routine,  a  clutter  weight,  a 
cut-off  for  very  small  value  weights,  processing  in  five-scan  batches,  a  maximum 
allowable  velocity  of  the  initial  state  estimate,  clutter  weight  model  inflation,  the  use  of 
the  Extended  Kalman  smoother,  and  the  use  of  attribute  data. 

The  results  from  this  research  have  shown  that  the  PMHT  algorithm  is  a  viable 
player  in  the  data  association  and  tracking  arena.  It  has  been  shown  to  outperform  the 
PDAF  in  all  of  the  scenarios  studied  here.  Furthermore,  it  has  proven  to  be  superior  to 
the  MHT  in  low  clutter  densities,  although  it  is  not  as  good  in  the  high  clutter  densities. 
The  PMHT  has  also  shown  that  it  can  track  a  target  through  a  minor  turn.  Even  though  it 
will  not  track  through  a  radical  target  maneuver,  this  is  not  a  glaring  weakness  since  most 
algorithms  require  special  processing  to  track  a  target  through  a  turn,  as  was  discussed  in 
Chapter  V. 

Presently,  the  algorithm’s  greatest  shortcoming  is  in  the  area  of  initialization.  The 
requirement  for  five  measurements  to  line  up  (N=5)  is  stringent,  especially  when  the 
probability  of  detection  is  less  than  one.  Unfortunately,  the  PMHT  algorithm  has  proven 
to  be  quite  sensitive  to  the  initial  estimates.  In  addition,  the  algorithm  is  also  easily 
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modified  to  make  use  of  attribute  data.  However,  the  current  lOdB  signal  to  noise  ratio  is 
rather  high,  and  probably  is  not  obtainable  in  the  underwater  sonar  world. 

B.  FURTHER  RESEARCH 

The  PMHT  algorithm  has  developed  quickly  since  it  was  proposed  by  Streit  and 
Luginbuhl  in  1995.  However,  there  are  still  several  areas  in  which  improvements  and 
further  research  need  to  be  addressed.  Clearly,  the  initialization  problem  needs 
development,  particularly  in  de-sensitizing  the  algorithm  to  the  initial  estimates. 

Another  area  for  further  research  is  the  use  of  attribute  data.  The  current  use 
offers  some  promise,  but  more  probability  research  needs  to  be  done  with  the  Rayleigh 
distribution  in  order  to  lower  the  lOdB  signal  to  noise  ratio.  Furthermore,  using  attribute 
data  during  initialization  needs  to  be  studied  more. 

The  final  area  where  more  research  is  needed  is  the  processing  of  the  algorithm 
during  radical  turns  and  maneuvers.  This  has  been  done  successfully  with  other  tracking 
algorithms,  but  actual  simulations  with  the  PMHT  algorithm  in  linking  tracks  together 
would  be  useful.  Investigation  of  the  requirements  necessary  for  the  new  estimates  after 
the  turn  would  be  important. 
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APPENDIX.  MATLABCODE 


This  appendix  contains  the  code  which  was  used  in  Matlab  4.2c’  to  simulate  the 
PMHT  algorithm.  The  first  set  of  code  is  for  the  two  crossing  target  scenario.  The 
second  set  of  code  is  for  a  straight  track  with  attribute  data.  For  the  other  scenarios,  slight 
modifications  were  made  to  either  of  these  programs. 


Crossing  Tracks 

%Probabilistic  Multi-Hypothesis  Tracking  (version  36) 

%  thesis  by  Capt.  Darin  T.  Dunham,  USMC 
%  advisor:  Prof.  R.  Gary  Hutchins,  NPS 
% 

%  two  tracks  with  clutter-- 
%  first  two  meas  are  one  std  error, 

%  the  other  meas  are  uniform  clutter. 

% 

%  Uses  clutter  tracking  model. 

% 

%  Tracks  in  Tstep  scan  increments. 

% 

%  Uses  attribute  data  for  each  measurement  NOT  IMPLEMENTED 
%  which  is  a  random  Rayleigh  distribution. 

% 

%  Tracks  cross. 

% 

%  Uses  first  5  points  to  initialize. 

% 

%  Uses  an  Extended  Kalman  instead  of  debiased  eqns. 

% 

%  Computes  the  mean  track  to  show  bias  of  second  track. 

% 


'  Matlab®  copyright©  1984-94  The  MathWorks,  Inc.,  All  Rights  Reserved,  Version  4.2c,  Nov.  23,  1994. 
The  MathWorks,  Inc.,  24  Prime  Park  Way,  Natick,  MA  01760. 
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clear 

tic 


%Constants 
M=3; 

N=5; 

Nt=2; 

Nc=10; 

T=30; 

Tstep=5; 
dt=4; 

1=100; 
sr=100; 
sth=pi/60; 
s=l; 
sc=l; 
q=l; 

maw  1=308.66667; 
maxv2=308. 66667; 
thr=l  0.5966; 
stop=le-6; 

J=500; 

%Constant  vectors 
c\U=le-10*[le-2  le-2  1111];  %clutter  model  weight 

%lnitiation  for  actual  track  (meters) 

Xlinit=[30200,  92.6,  30400,  -123.46667]’; 

X2init=[30200,  92.6,  16078,  123.46667]'; 

Yl=zeros(4,T); 

Y2=zeros(4,T); 

Yl(:.l)=Xlinit; 

Y2(:,l)=X2init; 

Q=q*[(dt^3)/3  (dt^2)/2  0  0;  (dt^2)/2  dt  0  0; 

0  0  (dt^3)/3  (dt^2)/2;  0  0  (dt^2)/2  dt]; 

R=[sr^2  0;  0  sth^2]; 

A=[0  1  0  0;  0  0  0  0;  0  0  0  1;  0  0  0  0]; 

C=[l  0  0  0;00  1  0]; 

Phi=eye(4)  +  A’'‘dt; 
invPhi=eye(4)  -  A*dt; 
fort=l:T-l, 

Yl(:,t+l)=Phi*Yl(:,t); 

Y2(:,t+l)=Phi*Y2(;,t); 


%three  models,  two  track,  one  clutter 
%number  of  points  used  to  initialize 
%number  of  tracks 
%number  of  clutter  points 
%number  of  scans 

%algorithm  increment  step  size  (change  cwt) 

%in  minutes 

%max  number  of  iterations 
%std  for  range 
%std  for  bearing 

%std  for  attribute  on  a  true  target 
%std  for  attribute  on  clutter 
%Q  coefficient 

%max  allowable  initial  velocity  for  track  1  (10  knots) 
%max  allowable  initial  velocity  for  track  2(10  knots) 
%threshold  for  3  std 
%convergence  stopping  parameter 
%number  of  loops  thru  the  simulation 
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end 

Y 1  pol=xy2polar(C*  Y 1 ); 
Y2pol=xy2polar(C*Y2); 

%Initialize  error  vectors 
ernn=zeros(l,T); 
erre=zeros(l,T); 
errm2=zeros(l  ,T); 
erre2=zeros(l  ,T); 
count!  =0; 
count2=0; 

%initialize  mean  estimated  track  vectors 
Xlm=zeros(4,T); 

X2m=zeros(4,T); 


forj=l:J, 

j 

%Initialize  tracker 

pl=0.2*ones(l,T);  %pl(t)-prob  that  target  1  has  a  meas  in  time  t 

p2=0.2*ones(l  ,T); 

p3=1.0*ones(l,T); 

%Generate  range  and  bearing  measurements 
Zlpol=Ylpol  +  sqrt(R)*randn(2,T); 

Z2pol=Y2pol  +  sqrt(R)*randn(2,T); 

%Convert  measurements  to  cartesian  using  Debiased  Eqns. 
mu=l  -  (exp(-sth^2)  -  exp(-(sth^2)/2)); 

Z 1  =(mu*eye(2))*polar2xy(Z  1  pol); 
Z2=(mu*eye(2))*poIar2xy(Z2pol); 

Rs=[convert(Z  1  pol,sr ,sth) ;  convert(Z2pol,sr,sth)] ; 

%Generate  attribute  data  for  targets 
Z=[ZlpoI;  raylmd(s,l,T);  Z2pol;  raylmd(s,l,T)]; 

%Generate  clutter  measurements 
for  m=l  :Nc, 

Zipol=xy2polar([l  .5e4*rand(l,T)+2.8e4;  2e4*rand(l  ,T)+1 .5e4]); 
Z=[Z;  Zipol;  raylmd(sc,l,T)]; 
end 
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i 

I 


%Build  Zbar 
Zbarl=Z(K:); 

Zbar2=Z(2,:); 

forv=4:3:3*(Nt+Nc), 

Zbarl=[Zbarl;  Z(v,:)]; 

Zbar2=[Zbar2;  Z(v+1,:)]; 
end 

%Initialize  XI  and  X2 
Xl=zeros(4,T); 

F=[C:  C*Phi;  C*(Phi^2);  C*(Phi'"3);  C*(PhiM)]; 

Rl=fomi(Rs(l:3J)); 

R2=form(Rs(l:3,2)); 

R3=fomi(Rs(  1:3,3)); 

R4=form(Rs(l  :3,4)); 

R5=fonn(Rs(l:3,5)); 

Sig=[Rl  zeros(2.8);  zeros(2,2)  R2  zeros(2,6); 

zeros(2,4)  R3  zeros(2,4);  zeros(2,6)  R4  zeros(2,2);  zeros(2,8)  R5]; 
invSig=inv(Sig); 

K=inv(F'*invSig*F)*F'*invSig; 

Zbar=[Zl(:,I);  Zl(:,2);  Zl(:,3)^;  Zl(:,4);  Zl(;,5)]; 

Xl(:,l)=K*Zbar; 

Pv  1  (:.2)=reshape((K*Sig*K’),  16,1); 

X2=zeros(4,T); 

Rl=form(Rs(4:6,l)); 

R2=form(Rs(4:6,2)); 

R3=form(Rs(4:6,3)); 

R4=form(Rs(4:6,4)); 

R5=fonn(Rs(4:6,5)); 

Sig=[Rl  zeros(2.8);  zeros(2,2)  R2  zeros(2,6); 

zeros(2,4)  R3  zeros(2,4);  zeros(2,6)  R4  zeros(2,2);  zeros(2,8)  R5]; 
invSig=inv(Sig); 

K=inv(F'*invSig*F)*F'*invSig; 

Zbar=[Z2(:.l);  Z2(:,2);  Z2(:,3)^;  Z2(:,4);  Z2(:,5)]; 

X2(:J)=K*Zbar; 

Pv2(:,2)=reshape((K*  Sig*K'),  16,1); 


%Limit  initial  velocity 
ininel  1  =sqrt(X  1  (2, 1  )^2  +  X 1  (4, 1  )^2); 
initvel2=sqrt(X2(2,l)^2  +  X2(4,l)^2); 
vfactorl  =initvel  1  /maxvl ; 
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vfactor2=initvel2/maxv2; 
if  vfactorl>l, 

Xl(2,l)=Xl(2,l)/vfactorl; 

XI  (4, 1)=X1  (4, 1  )/vfactor  1 ; 
end 

if  vfactor2>l, 

X2(2,l)=X2(2,l)/vfactor2; 

X2(4, 1  )=X2(4, 1  )/vfactor2; 
end 

%Predict  initial  estimate  for  first  five  points 
for  t=2:5, 

Xl(:,t)=Phi*Xl(:,t-l); 

X2(:,t)=Phi*X2(:,t-l); 

Pvl(:,2*t)=reshape((Phi*fonnP(Pvl(:,2*(t-l)))*Phi'  +  Q),16,l); 
Pv2(:,2*t)=reshape((Phi*formP(Pv2(:,2*(t-l)))*Phi'  +  Q),16,l); 
end 

U1=X1(:,1:5); 

U2=X2(:,1:5); 

%Tra.ck  in  five  scan  increments* ************************************ 
for  Ti=5;Tstep:T, 

%Begin  iterations 
for  i=l  :I, 

%store  last  target  meas  prob 

plp=pl; 

p2p=p2; 

p3p=p3; 

for  t=l  :Ti, 
n(t)=Nt+Nc; 

%compute  weights 
for  r=l;n(t), 

zt=Z((3  *r-2):(3*r- 1  ),t)-xy2polar(C*X  1  (:,t)); 

Ck=Cekf(Xl(:,t)); 

Sig=Ck*reshape(Pvl(:,2*t),4,4)*Ck'  +  R; 
den=2*pi*  sqrt(det(Sig)); 
wl  (r,t)=exp(-0. 5  *  zt'  *inv(Sig)  *  zt)/ den; 
if  zt'*inv(Sig)*zt  >  thr, 
wl(r,t)=le-20; 
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end 


zt=Z((3*r-2):(3*r-l),t)-xy2polar(C*X2(:,t)); 

Ck=Cekf(X2(:d)); 

Sig=Ck*reshape(Pv2(:,2*t),4,4)*Ck'  +  R; 
den=2*pi*sqrt(det(Sig)); 
\v2(r,t)=exp(-0.5*zt'*inv(Sig)*zt)/den; 
if  zt'*inv(Sig)*zt  >  thr, 
w2(nt)=le-20; 
end 

\v3(r,t)=cwt(Ti/Tstep); 

suml  =(pl  (t)*wl  (r,t)+p2(t)*w2(r,t)+p3(t)*w3(r,t)); 
w  1  (r,t)=w  1  (r,t)/sum  1 ; 
w2(r.t)=w2(r,t)/sum  1 ; 

\v3(r,t)=w3(r,t)/suml ; 
end 

%compute  mean  meas  weight  for  target  m  at  time  t 
wlm(t)=(l/n(t))*sum(wl(:,t)); 
w2m(t)=(l/n(t))*sum(w2(:,t)); 
w3  m(t)=(  1 /n(t))*  sum(w3  ( :  ,t)); 

%update  target  meas  prob 
pl(t)=wlm(t)*pl(t); 
p2(t)=\v2m(t)*p2(t); 
p3(t)=w3m(t)*p3(t); 

%compute  target  meas  centroid 
W 1  =\v  1  ( ;  ,t)/(n(t)  *  w  1  m(t)); 
M'2=w2(:,t)/(n(t)*w2m(t)); 
Zlhat(:.t)=[Wl'*Zbarl(:,t);  Wl’*Zbar2(:,t)]; 
Z2hat(:d)=[W2’*ZbarI(:,t);  W2'*Zbar2(;,t)]; 

end 

%run  Extended  Kalman  smoother 

ylhat(:J)=Xl(;,l); 

y2hat(:.l)=X2(:,l); 


%for\vard  recursion 
for  t=l;Ti-l, 

Pt=Phi*reshape(Pvl(:,2*t),4,4)*Phi'  +  Q; 
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Pvl(:,2*t+l)=Pt(:); 

y  1  hat(:,t+ 1  )=Phi*y  1  hat(:  ,t); 

k=n(t+l)*pl(t+l); 

Ck=Cekf(y  1  hat( :  ,t+ 1 )); 

G=k*Pt*Ck’*inv(k*Ck*Pt*Ck'  +  R); 

Pv  1  (:  ,2*t+2)=reshape((Pt-G*Ck*Pt),  16,1); 

ylhat(;,t+l)=ylhat(:,t+l)+G*(Zlhat(:,t+l)-xy2polar(C*ylhat(:,t+l))); 

Pt=Phi*reshape(Pv2(:,2*t),4,4)*Phi'  +  Q; 

Pv2(:,2*t+l)=Pt(:); 
y  2hat( :  ,t+ 1  )=Phi  *y2hat( :  ,t) ; 
k=n(t+l)*p2(t+l); 

Ck=Cekf(y2hat(:,t+ 1 )); 

G=k*Pt*Ck'*inv(k*Ck*Pt*Ck'  +  R); 
Pv2(:,2*t+2)=reshape((Pt-G*Ck*Pt),  16,1); 

y2hat(:,t+l)^2hat(:,t+l)+G*(Z2hat(:,t+l)-xy2polar(C*y2hat(:,t+l))); 

end 

Xl(:,Ti)=ylhat(:,Ti); 

X2(:,Ti)=y2hat(:,Ti); 

%backward  recursion 
fort=Ti-l;-l:l, 

Ptt=reshape(Pvl  (:,2*t),4,4); 

invPt=inv(reshape(Pvl  (:,2*t+l  ),4,4)); 

Xl(:,t)=ylhat(:,t)+Ptt*Phi'*invPt*(Xl(:,t+l)-Phi*ylhat(:,t)); 

Ptt=reshape(Pv2( :  ,2  *t),4,4); 
invPt=inv(reshape(P  v2( :  ,2  *  t+ 1  ),4,4)); 
X2(:,t)=y2hat(:,t)+Ptt*Phi'*invPt*(X2(;,t+l)-Phi*y2hat(:,t)); 
end 

%check  for  convergence 

diff=sum(abs(pl-plp))+sum(abs(p2-p2p))+sum(abs(p3-p3p)); 
if  diff<stop, 
break 
end 

end 


%plot  track  output 
figure(l) 

plot(Yl(l,l:Ti),Yl(3,l:Ti),Xl(l,l:Ti),Xl(3,l:Ti),Y2(l,l:Ti),... 
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Y2(3 , 1  :Ti),X2(  1 , 1  ;Ti),X2(3, 1  :Ti)) 
axis('equal’),  title(Tracking  Algorithm  Output') 
xlabel('x  (meters)'),  ylabel('y  (meters)') 

%Predict  for  next  Tstep  points  if  necessary 
ifTi<T, 

mid=floor(Ti/2); 

init\-ell=sqrt(Xl(2,mid)^2  +  Xl(4,mid)^2); 
inin-el2=sqrt(X2(2,mid)'^2  +  X2(4,mid)^2); 
vfactorl=initvell/maxvl ; 
vfactor2=init\'el2/maxv2; 
if  vfactorl>l, 

X 1  (2,mid)=X  1  (2,mid)/vfactorl ; 

X 1  (4,mid)=Xl  (4,mid)/vfactorl ; 
end 

if  vfactor2>l, 

X2(2,mid)=X2(2,mid)/vfactor2; 

X2(4,mid)=X2(4,mid)/vfactor2; 

end 

for  t=mid+l  :Ti+Tstep, 

Xl(;,t)=Phi*Xl(:,t-l); 

Pvl(:,2n)=reshape((Phi*reshape(Pvl(:.2*(t-l)),4,4)*Phi'+Q),16.1); 

X2(:.t)=Phi*X2(;,t-l); 

Pv2(:,2n)=reshape((Phi*reshape(Pv2(:,2*(t-l)),4,4)*Phi'+Q),16,l); 

end 

for  t=mid-l:-I:l, 

Xl(;,t)=invPhi*Xl(:,t+l); 

X2(:.t)=invPhi*X2(;,t+l); 

end 

end 


%update  measurement  &  estimate  errors  for  track  1 
errx=Yl(l.:)-Zl(l,;); 
em-=Yl(3,:)-Zl(2,:); 
ernn=sqrt(errx.'''2  +  err>'.^2)./J  +  entn; 
errx=Yl(l,:)-X  1(1,0; 
eiT>=Yl(3,0-Xl(3,0; 
errej=sqrt(eiTX.^2  +  eny.^2); 
erre=erTej./J  +  erre; 
if  max(errej)  >  2000, 
count  l=count  1+1; 
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end 


%update  measurement  &  estimate  errors  for  track  2 
errx=Y2(l,:)-Z2(l,:); 
erry=Y2(3,:)-Z2(2,:); 
errm2=sqrt(errx.^2  +  erry.^2)./J  +  errm2; 
errx=Y2(l,:)-X2(l,:); 
erry=Y2(3,:)-X2(3,:); 
errej=sqrt(errx.''2  +  erry.'^2); 
erre2=errej./J  +  erre2; 
if  max(errej)  >  2000, 
count2=coimt2+l ; 
end 


%update  mean  estimated  tracks 
Xlm=Xlm  +  XL/J; 
X2m=X2m  +  X2./J; 


end 

%End  J  times 


loop 


%plot  errors 
figure(2) 
load  kserror 

plot(l  :T,errm,l  :T,erre,l  :T,errm2,l  :T,erre2,l  :T,errl(l  :T),1  ;T,err2(l  :T)) 
title('Measurement  Error  &  Estimate  Error,  over  100  runs') 
xlabel('time'),  ylabel('distance  (m)') 

figure(3) 

plot(Yl(l,l:T),Yl(3,l:T),Xlm(l,l:T),Xlm(3,l:T),Y2(l,l:T),... 

Y2(3, 1  :T),X2m(l  ,1  :T),X2m(3, 1  :T)) 
axis('equar),  title('Tracking  Algorithm  Output') 
xlabel('x  (meters)'),  ylabel('y  (meters)') 

time=toc/60; 

[time  count  1  count2] 


Attribute  Data 

%Probabilistic  Multi-Hypothesis  Tracking  (version  35) 
%  thesis  by  Capt.  Darin  T.  Dunham,  USMC 
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%  advisor:  Prof.  R.  Gar}'  Hutchins,  NPS 

0/ 

/o 

%  one  track  with  clutter— 

%  first  meas  is  one  std  erroi 

5 

%  the  other  meas  are  uniform  clutter. 

/O 

%  Uses  clutter  tracking  model. 

/O 

%  Tracks  in  Tstep  scan  increments. 

O/n 

/o 

%  Uses  attribute  data  for  each  measurement 

%  which  is  a  random  Rayleigh  distribution. 

0/ 

/o 

%  Uses  first  5  points  to  initialize. 

0/1 

/u 

%  Uses  an  Extended  Kalman 

0  / 

70 

instead  of  debiased  eqns. 

%clear 

tic 

%Constants 

M=2; 

%two  models 

N=5; 

%number  of  points  used  to  initialize 

Nt=l; 

%number  of  tracks 

Nc=20; 

%number  of  clutter  points 

T=30; 

%number  of  scans 

Tstep=5; 

%algorithm  increment  step  size 

dt=4; 

%in  minutes 

1=100; 

%max  number  of  iterations 

sr=I00; 

%std  for  range 

sth=pi'60; 

%std  for  bearing 

%s=sqrt(10); 

%std  for  attribute  on  a  true  target 

sc=l; 

%std  for  attribute  on  clutter 

%Q  coefficient 

maxv=308. 66667; 

%max  allowable  predict  velocity  (lO  knots) 

thr=l  0.5966; 

%threshold  for  3  std 

stop=le-8; 

%convergence  value  for  Pi 

J=500; 

%number  of  loops  thru  the  simulation 

%Constant  vectors 

c\\l-le-l0*[le-2  le-2  I  I  I  I];  %clutter  model  weight 
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%iiiitiation  for  actual  track  (meters) 

Xlinit=[30200,  92.6,  30400,  -123.46667]'; 

%Xlinit=[30200,  92.6, 16078,  123.46667]'; 

Yl=zeros(4,T); 

Yl(:,l)=Xlinit; 

C=[l  0  0  0;  0  0  1  0]; 

Q=q*[(dt^3)/3  (dt^2)/2  0  0;  (dt^2)/2  dt  0  0; 

0  0  (dt^3)/3  (dt^2)/2;  0  0  (dt^2)/2  dt]; 

R=[sr^2  0;  0  sth^2]; 

A=[0  1  0  0;  0  0  0  0;  0  0  0  1;  0  0  0  0]; 

Phi=eye(4)  +  A*dt; 
invPhi=eye(4)  -  A*dt; 
fort=l:T-l, 

Yl(:,t+l)=Phi*Yl(;,t); 

end 

Y 1  pol=xy2polar(C*  Y 1 ); 
errm=zeros(l,T); 
erre=zeros(l,T); 
coimt=0; 

%Loop  through  simulation  J  times* ************************************** 
forj=l:J, 

j 

%Initialize  tracker 

pl=0.2*ones(l,T);  %pl(t)-prob  that  tar  1  has  a  meas  in  time  t 
p2=l  .0*ones(l  ,T); 

%Generate  range  and  bearing  measurements 
Zlpol=Ylpol  +  sqrt(R)*randn(2,T); 

%Convert  measurements  to  cartesian  using  Debiased  Eqns. 
mu=l  -  (exp(-sth^2)  -  exp(-(sth^2)/2)); 

Z 1  =(mu*  eye(2))*  (polar2xy(Z  1  pol)) ; 

Rs=convert(Zl  pol,sr,sth); 

%Generate  attribute  data  for  target  &  measurement  covariance 
Z=[Zlpol;  raylmd(s,l,T)]; 

%Generate  clutter  measurements 
for  m=l  :Nc, 

Zipol=xy2polar([1.5e4*rand(l,T)+2.8e4;  2e4*rand(l,T)+1.5e4]); 

Z=[Z;  Zipol;  raylmd(sc,l,T)]; 
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end 


%Build  Zbar 
Zbarl=Z(K:); 

Zbar2=Z(2,:); 
for  \-=4:3;3*(Nt+Nc), 

Zbarl=[Zbarl;  Z(v,:)]; 

Zbar2=[Zbar2;  Z(v+1,:)]; 
end 

%Initialize  X 
Xl=zeros(4,T); 

F=[C;  C*Phi;  C*(Phi^2);  C*(Phi^3);  C*(PhiM)]; 

Rl=form(Rs(:,l)); 

R2=form(Rs(:,2)); 

R3=form(Rs(;.3)); 

R4=form(Rs(:,4)); 

R5=form(Rs(:,5)); 

Sig=[Rl  zeros(2,8);  zeros(2,2)  R2  zeros(2,6); 

zeros(2,4)  R3  zeros(2.4);  zeros(2,6)  R4  zeros(2,2);  zeros(2,8)  R5]; 
invSig=inv(Sig); 

K=inv(F'*invSig*F)*F'*invSig; 

Zbar=[Zl(;.l);  Zl(:,2);  Z1(:J);  Zl(:,4);  Zl(:,5)]; 

Xl(:.l)=K*Zbar; 

Pv(:,2)=reshape((K*Sig*K'),  1 6, 1 ); 

%Limit  initial  velocity 
iniKel=sqrtp(l(2,l)''2  +  XI  (4, 1)^2); 
vfactor=initvel/maxv; 
if  vfactor>  1 , 

XK2.1)=Xl(24)/vfactor; 

Xl(4,l)=Xl(4,l)/vfactor; 

end 

%Predict  initial  estimate  for  first  five  points 
for  t=2:5. 

Xl(:.t)=Phi*Xl(:4-l); 

Pv(:,2n)=reshape((Phi*formP(Pv(:,2*(t-l)))*Phi’  +  Q),16,l); 

end 

U1=X1(:.1:5); 

%Track  in  Tstep  scan  increments************************************* 
for  Ti=5:Tstep:T, 


64 


%Begin  iterations 
for  i=l  :I, 


%store  last  target  meas  prob 

plp=pl; 

p2p=p2; 

for  t=l  :Ti, 
n(t)=Nt+Nc; 

%compute  weights 
for  r=l:n(t), 

zt=Z((3  *r-2):(3  *r- 1  ),t)-xy2polar(C*Xl  (:,t)); 
Ck=Cekf(Xl(:,t)); 

Sig=Ck*reshape(Pv(:,2*t),4,4)*Ck'  +  R; 
den=2*pi*sqrt(det(Sig)); 

wl(r,t)=raylpdf(Z(3*r,t),s)*exp(-0.5*zt'*inv(Sig)*zt)/den; 
if  zt'*inv(Sig)*zt  >  thr, 
wl(r,t)=le-20; 
end 

w2(r,t)=raylpdf(Z(3*r,t),sc)*cwt(Ti/Tstep); 
sum  1  =(p  1  (t)*  wl  (r,t)+p2(t)*  w2(r,t)); 
wl  (r,t)=wl  (r,t)/suml ; 
w2(r,t)=w2(r,t)/suml ; 
end 

%compute  mean  meas  weight  for  target  m  at  time  t 
wl  m(t)=(l/n(t))*sum(wl  (:,t)); 
w2m(t)=(l/n(t))*sum(w2(:,t)); 

%update  target  meas  prob 

pl(t)=wlm(t)*pl(t); 

p2(t)=w2m(t)*p2(t); 

%compute  target  meas  centroid 
W=wl  (:,t)/(n(t)*  wl  m(t)); 

Zhat(:,t)=[W'*Zbarl (:,t);  W’*Zbar2(:,t)] ; 

end 


%run  Extended  Kalman  smoother 
yhat(:,l)=Xl(:,l); 
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%for\vard  recursion 
for  t=l;Ti-L 

Pt=Phi*reshape(Pv(:,2*t),4,4)*Phi'  +  Q; 

Pv(:,2*t+l)=Pt(;); 
yhat( :  ,t+ 1  )=Phi  *  yhat( :  ,t); 
k=n(t+l)*pl(t+l); 

Ck=Cekf(yhat(:,t+l)); 

G=k*Pt*Ck'*inv(k*Ck*Pt*Ck’  +  R); 
Pv(:,2*t+2)=reshape((Pt-G*Ck*Pt),16,l); 
yhat( :  ,t+ 1  )=yhat( :  ,t+ 1  )+G*  (Zhat(;  ,t+ 1  )-xy2polar(C*  yhat( :  ,t+ 1 ))); 
end 

Xl(:.Ti)=yhat(;,Ti); 

%backward  recursion 
for  t=Ti-l:-I:l, 

Pu=reshape(Pv( :  ,2  *  t),4,4); 

invPt=inv(reshape(Pv(:,2*t+l),4,4)); 

Xl(:,t)=yhat(:,t)+Ptt*Phi'*invPt*(Xl(;,t+l)-Phi*yhat(:,t)); 

end 

%check  for  convergence 
diff=sum(abs(pl  -p  1  p))+sum(abs(p2-p2p)); 
if  diff  <  stop, 
break 
end 

end 

U2=Xl(:.l:Ti); 


%plot  track  output 
figure(l) 

piotC  Y  1(1,1  :Ti),  Y1  (3, 1  :Ti),X  1(1,1  ;Ti),X  1(3,1  :Ti)) 
title('Tracking  Algorithm  Output') 
xlabel('x  (meters)'),  ylabel('y  (meters)') 

%Predict  for  next  five  points  if  necessary 
ifTi<T, 

mid=floor(Ti/2); 

initvel=sqrt(Xl(2,mid)^2  +  Xl(4,mid)^2); 

vfactor=initvel/maxv; 

if  vfactor>l. 
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X 1  (2,mid)=X  1  (2,mid)/vfactor; 

X 1  (4,mid)=X  1  (4,mid)/vfactor; 
end 

for  t=mid+l  :Ti+Tstep, 

Xl(:,t)=Phi*Xl(:,t-l); 

Pv(;  ,2*t)=reshape((Phi*reshape(Pv( :  ,2*  (t- 1  )),4,4)*Phi'  +  Q),  1 6, 1 ); 
end 

fort=mid-l:-l:l, 

Xl(:,t)=invPhi*Xl(:,t+l); 

end 

end 

end 

errx=Yl  ( 1 ,  :)-Z  1  ( 1 , 
erry=Yl(3,:)-Zl(2,:); 
errm=sqrt(errx.^2  +  erry.^2)./J  +  errm; 
errx=Yl(l,:)-Xl(l,:); 
erTy=Yl(3,:)-Xl(3,:); 
errej=sqrt(errx.^2  +  erry.'^2); 
erre=errej./J  +  erre; 
if  max(errej)  >  2000, 
count=count+l ; 
end 

end 

%End  J  times  jQop*************************************************** 

%plot  errors 
figure(2) 
load  kserror 

plot(l  ;T,erre,l  :T,errl(l  ;T),1  :T,err2(l  :T)) 
title('Measurement  Error  &  Estimate  Error,  over  100  runs') 
xlabel('time'),  ylabel('distance  (m)') 

vel=sqrt(U  1(2, 1)^2  +  Ul(4,l)^2); 

anglediff^(  1 80/pi)*(atan2(Ul  (4, 1  ),U1  (2, 1  ))-atan2(Yl  (4, 1  ),Y1  (2, 1 ))); 
time=toc/60; 

[vel  anglediff  time  coimt] 
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